#!/usr/bin/env python3
# coding=utf-8

import sys
import re
import rospy
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Float32MultiArray
from nlink_parser.msg import LinktrackAoaNodeframe0

from matplotlib import pyplot as plt
import numpy as np
import math





# def callback(msg):
#     dis = msg.psd01
#     x_idx=x_idx+1
#     xdata.append(x_idx)	# 添加i到x轴的数据中
#     ydata.append(dis)	# 添加i的平方到y轴的数据中

def plot_it():
    rospy.init_node("plot_obs_grid", anonymous=True)  
    rospy.loginfo("Plot_obs_grid running...")

    # rospy.Subscriber("/psd_ultrasonic",ultrasonic_msg,callback)

    plt.ion() #开启interactive mode 成功的关键函数
    figure, ax = plt.subplots(figsize=(8,8))
    # fig = plt.figure(figsize=(10, 5))  # 创建图
    plt.ylim(0, 3.5)  # Y轴取值范围
    plt.xlim(0, 4.0)  # X轴取值范围
    plt.title("obs_grid")   # 标题
    plt.xlabel("X")  # X轴标签
    plt.ylabel("Y")  # Y轴标签
  

    rate = rospy.Rate(100) # 20:20hz
    count = 0

    # 创建实时绘制横纵轴变量

    try:

        while not rospy.is_shutdown():
            msg1 = rospy.wait_for_message("/obstacle_grid", Int32MultiArray, timeout=None)
            msg2 = rospy.wait_for_message("/obstacle_angle_grid", Float32MultiArray, timeout=None)
            msg3 = rospy.wait_for_message("/nlink_linktrack_aoa_nodeframe0", LinktrackAoaNodeframe0, timeout=None)
            # print(len(msg1.data))  # len(msg1.data) 5625
            # print(len(msg2.data))
            # print(msg3.nodes[0])
        
            xdata, ydata = [], []  # 用于保存绘图数据，最开始时什么都没有，默认为空
            xdata1, ydata1 = [], []  # 用于保存绘图数据，最开始时什么都没有，默认为空
            xdata2, ydata2 = [], []  # 用于保存绘图数据，最开始时什么都没有，默认为空

            for i in range(len(msg1.data)):
                if msg1.data[i]>0:                    
                    ydata1.append((i % 75) * 4.0 / 100.0)
                    xdata1.append((i / 75) * 4.0 / 100.0)
            # print(ydata1)
            # print(xdata1)

            # xdata = [i * 0.2 * 3.1415926 / 180 for i in range(len(msg2.data))]
            # ydata = msg2.data
            for i in range(len(msg2.data)):
                if msg2.data[i]>0:                    
                    ydata.append(msg2.data[i] * math.cos(i * 0.2 * math.pi / 180 - math.pi /2))
                    xdata.append(msg2.data[i] * math.sin(i * 0.2 * math.pi  / 180 - math.pi /2) + 1.5)

            if len(msg3.nodes)>0:
                dis = msg3.nodes[0].dis
                angle = msg3.nodes[0].angle
                xdata2.append(1.5)
                ydata2.append(0)
                ydata2.append(dis * math.cos(angle * math.pi / 180 ))
                xdata2.append(dis * math.sin(angle * math.pi  / 180) + 1.5)


            count+=1
            if count==2:
                count = 0
                plt.cla()
                plt.clf()
                plt.ylim(0, 3.0)  # Y轴取值范围
                plt.xlim(3.0,0)  # X轴取值范围

                plt.plot(xdata2, ydata2, "r--",lw=1.5)  # 绘制折线图
                plt.plot(xdata, ydata, ".",markersize=0.2,color='b')  # 绘制折线图
                plt.plot(xdata1, ydata1, "o",markersize=3,color='r')  # 绘制折线图
                plt.draw()#注意此函数需要调用
                figure.canvas.flush_events()

            rate.sleep()

    except Exception as e:
        rospy.loginfo("ex:%s",e)
        pass

    # rospy.loginfo("X min,max= %.2f,%.2f; Y min,max=  %.2f,%.2f",min(xdata),max(xdata),min(ydata),max(ydata))    
    # plt.savefig('mcar1.png', dpi=300, bbox_inches='tight')
    # rospy.loginfo("plt saved to mcar1.png")


if __name__ == '__main__':
    plot_it()
